package com.yukefms.engine.senario;

import com.yukefms.jointpoint.Kinect25JointType;
import com.yukefms.jointpoint.TolerantEpsilon;
import com.yukefms.rule.TriggerResponseBARule;
import com.yukefms.rule.TriggerStageBARule;
import com.yukefms.rule.formula.SeqStageBAFormula;
import com.yukefms.rule.formula.SimpleStageBAFormula;
import com.yukefms.rule.formula.StaticBAFormulaChain;
import com.yukefms.rule.formula.basis.*;
import com.yukefms.rule.formula.interfaces.DynamicBAFormula;
import com.yukefms.rule.formula.interfaces.StageBAFormula;
import com.yukefms.rule.formula.interfaces.StaticBAFormula;
import com.yukefms.rule.interfaces.BARule;
import com.yukefms.rule.interfaces.BAStageRule;

/**
 * @Description
 * @Author Ping
 * @Date 2021/8/19 15:34
 **/
public class RightCrossLineSenario implements Senario {
    public static final String RULE_NAME = "RIGHT_CROSS_LINE" ;
    /**
     * the parameter for raising. More larger, more easy to reach
     * The distance between one ankle and other knee.
     */
    public static final double ABOVE_LENGTH_DISTANCE = 0.15;
    /**
     * the minimum distance in z for crossing action.
     * More larger is harder to reach.
     *      0.008
     */
    public static final double CROSSING_LENGTH_Z = 0.0006 ;
    /**
     * the distance range for foot and ankle.
     * More larger, more easy to reach.
     * 0.15
     */
    public static final double FOOT_TOGETHER_EPSILON = 0.15 ;




    @Override
    public BAStageRule buildRule(String ruleName) {
        TriggerStageBARule.TriggerStageRuleBuilder ruleBuilder = TriggerStageBARule.builder() ;
        /*
        *//**
         * Trigger Part
         *
         */

        StaticBAFormulaChain trigger = new StaticBAFormulaChain() ;
        trigger.setScore(1);
        /**
         * (1) Two feet stand together
         * (2) Two hands and shoulder are in parallel
         * (3) Stand straight
         */
        trigger.add(
                // two feet stand together
                new TwoPointsNearbyFormula(
                        Kinect25JointType.JointType_FootLeft.toString(),
                        Kinect25JointType.JointType_FootRight.toString(),
                        TolerantEpsilon.FOOT_TOGETHER_EPSILON
                ))
                // two hands and shoulder are in parallel
                .add(new InParallelFormula("4","8", "6","10"))
                .add(new InLine2DFormula(
                        Kinect25JointType.JointType_HandLeft.toString(),
                        Kinect25JointType.JointType_Neck.toString(),
                        Kinect25JointType.JointType_HandRight.toString()
                ))
                // left three leg points in line
                .add(new InLineFormula(
                        Kinect25JointType.JointType_KneeLeft.toString(),
                        Kinect25JointType.JointType_HipLeft.toString(),
                        Kinect25JointType.JointType_AnkleLeft.toString()))
                // stand straight with the line of spineMid and shouldMid in parallel with
                //  the line of hip and knee
                .add(new InParallelFormula(
                        Kinect25JointType.JointType_SpineMid.toString(),
                        Kinect25JointType.JointType_SpineShoulder.toString(),
                        Kinect25JointType.JointType_AnkleRight.toString(),
                        Kinect25JointType.JointType_KneeRight.toString()

                ) )
                // right three leg points in line
                .add(new InLineFormula(
                        Kinect25JointType.JointType_KneeRight.toString(),
                        Kinect25JointType.JointType_HipRight.toString(),
                        Kinect25JointType.JointType_AnkleRight.toString()));
        trigger.setName("STAND WELL TO START");
        trigger.setScore(1) ;
        trigger.setDescription("[TO MAKE] Stand_Straight\nHand_Shoulder_In_Line\nTwo_Feet_Together");
        /**
         * Performing Part
         *
         */
        StageBAFormula stages = new SeqStageBAFormula() ;
        // Stage1: Raising Right Leg to certain high level
        StageBAFormula raisingLeg = new SimpleStageBAFormula(
                // Dynamic Part: raising right leg
                new MovingUpDynamicBAFormula(Kinect25JointType.JointType_AnkleRight.toString()),
                // Response Part: above left knee
                new HorizonalBelowFormula(
                        Kinect25JointType.JointType_KneeLeft.toString(),
                        Kinect25JointType.JointType_AnkleRight.toString(),
                        // important parameter
                        ABOVE_LENGTH_DISTANCE
                )
        ) ;
        raisingLeg.setName("Raising");
        raisingLeg.dynamic().setName("Raising");
        raisingLeg.response().setName("ToHightLevel" );
        raisingLeg.setScore(1);
        // Stage 2:  Crossing line and going down .
        StageBAFormula crossingLine =  new SimpleStageBAFormula(
                // Dynamic Part: Crossing the line
                new DoingToUnSatBAFormula(
                        Kinect25JointType.JointType_AnkleRight.toString(),
                        CROSSING_LENGTH_Z
                ),

                // Response part: going down
                // NOTE THAT, there is a conflict between Crossing and Going down,
                // the Z is discreasing in Crossing, but is increasing in going down.
                new MovingDownDynamicBAFormula(
                        Kinect25JointType.JointType_AnkleRight.toString()
                )
        ) ;

        crossingLine.setName("Crossing");
        crossingLine.dynamic().setName("Crossing");
        crossingLine.response().setName("GoingDown" );
        crossingLine.setScore(2);
        // Stage 3:  Going down the right leg to ground.
        StageBAFormula goingDown = new SimpleStageBAFormula(
                // Dynamic Part: moving down the right leg
                new MovingDownDynamicBAFormula(
                        Kinect25JointType.JointType_AnkleRight.toString()
                ),
                // Response part: Right ankle is at front of left foot
                new AtFrontFormula(
                        Kinect25JointType.JointType_AnkleRight.toString() ,
                        Kinect25JointType.JointType_FootLeft.toString(),
                        FOOT_TOGETHER_EPSILON
                )
        ) ;
        goingDown.setName("GoingDown");
        goingDown.dynamic().setName("GoingDown");
        goingDown.response().setName("ToGround" );
        goingDown.setScore(2);


        stages.add(raisingLeg)
                .add(crossingLine)
                .add(goingDown) ;
        stages.setName("RaiseCrossDOWN") ;
        /**
         * Invariant part
         */
        StaticBAFormulaChain invarint = new StaticBAFormulaChain() ;
        invarint.add(
                // (1) SpineMid does not move too much.
                new KeepNoMovingDynamicBAFormula(Kinect25JointType.JointType_SpineMid.toString()) )
                // (2) hand and shoulder keep right position
                .add(new InParallelFormula("4","8", "6","10"))
                .add(new InLine2DFormula(
                        Kinect25JointType.JointType_HandLeft.toString(),
                        Kinect25JointType.JointType_Neck.toString(),
                        Kinect25JointType.JointType_HandRight.toString()
                ))
                // (3) two legs keep parallel during moving.
                .add(new In2DParallelFormula(
                        Kinect25JointType.JointType_AnkleRight.toString(),
                        Kinect25JointType.JointType_KneeRight.toString(),
                        Kinect25JointType.JointType_AnkleLeft.toString(),
                        Kinect25JointType.JointType_KneeLeft.toString()

                ));
        invarint.setName("KeepStatic");
        invarint.setDescription("[TO MAKE] Keep_Static\nHand_Shoulder_In_Line\nLegs_In_Parallel");

    /**
     *  Build rule
     * */
        BAStageRule rule = ruleBuilder.setTrigger(trigger)
                .setStage(stages)
                .setInvariant(invarint)
                .setRuleName(ruleName)
                .build() ;
        rule.setSuccessScore(3);

        return rule;
    }
}
